﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace Engine.Physics
{
   /// <summary>
    /// Compnent that enables physically correct movement and interaction with force fields
    /// </summary>
    public class Rigidbody : Component
    {
        #region fields
        //Mass of the object
        public float Mass;

        public float Density = 78;

        //Coefficient of Restitution, how "bouncy" is the object
        public float COR = 1;

        //How much energy is converted into rotational movement
        public float Inertia = 10000;

        //Is the object accelerated by gravity?
        public bool IsAffectedByGravity = true;

        //Flag set to true in between StartIntegration and EndFrame
        private bool inFrame = false;
        
        //Velocity of the object
        public Vector2 Velocity { get; private set; }

        //Acceleration of the object in the current frame
        private Vector2 acceleration;

        public Vector2 OwnPosition { get { return inFrame ? newPosition : Position; } }
        public Vector2 newPosition;
        public Vector2 newVelocity;
        private Vector2 newAcceleration;

        public float AngularVelocity { get; private set; }
        private float angularAcceleration;
        
        public float OwnRotation { get { return inFrame ? newRotation : gameObject.transform.Rotation; } }
        public float newRotation;
        public float newAngularVelocity;
        private float newAngularAcceleration;
        #endregion

        #region constructors
        public Rigidbody()
        {
            Mass = 1;
            IsActive = true;
            Velocity = new Vector2();
            acceleration = new Vector2();

            ownPos = new Vector2();
            newPosition = new Vector2();
            newVelocity = new Vector2();
            newAcceleration = new Vector2();
        }

        public Rigidbody(float _mass)
        {
            Mass = _mass;
            IsActive = true;

            ownPos = new Vector2();
            Velocity = new Vector2();
            acceleration = new Vector2();

            newPosition = new Vector2();
            newVelocity = new Vector2();
            newAcceleration = new Vector2();

            newRotation = 0;
            newAngularVelocity = 0;
            newAngularAcceleration = 0;
        }

        public Rigidbody(float _mass, bool _affectedByGravity) : this(_mass)
        {
            IsAffectedByGravity = _affectedByGravity;
        }

        public Rigidbody(float _mass, float _cor) : this(_mass)
        {
            COR = _cor;
        }

        public Rigidbody(float _mass, float _cor, float _inertia) : this(_mass, _cor)
        {
            Inertia = _inertia;
        }

        public Rigidbody(float _mass, float _cor, float _inertia, bool _affectedByGravity)
            : this(_mass, _cor)
        {
            Inertia = _inertia;
            IsAffectedByGravity = _affectedByGravity;
        }

        #endregion

        public override void OnAdd(GameObject _gameObject)
        {
            base.OnAdd(_gameObject);
            _gameObject.rigidbody = this;
            newPosition = gameObject.transform.WorldPosition;
            PhysicsEngine.Instance.AddRigidbody(this);
        }

        public override void OnRemove()
        {
            gameObject.rigidbody = null;
            PhysicsEngine.Instance.RemoveRigidbody(this);
            base.OnRemove();
        }

        public override void Awake()
        {
            /*Inertia = CalcInertia();
            Mass = CalcMass();*/
        }

        private float CalcMass()
        {
            Rectangle aabb = gameObject.collider.AABB;
            aabb.X -= (int)Position.X;
            aabb.Y -= (int)Position.Y;
            List<Vector2> points = new List<Vector2>();
            points.Add(new Vector2(aabb.X, aabb.Y));
            points.Add(new Vector2(aabb.X + aabb.Width, aabb.Y));
            points.Add(new Vector2(aabb.X + aabb.Width, aabb.Y + aabb.Height));
            points.Add(new Vector2(aabb.X, aabb.Y + aabb.Height));
            return GeometryUtils.GetArea(points) * Density/100;
        }

        private float CalcInertia()
        {
            if (gameObject.collider == null)
                return 10000;

            Rectangle aabb = gameObject.collider.AABB;
            aabb.X -= (int)Position.X;
            aabb.Y -= (int)Position.Y;
            List<Vector2> points = new List<Vector2>();
            points.Add(new Vector2(aabb.X, aabb.Y));
            points.Add(new Vector2(aabb.X + aabb.Width, aabb.Y));
            points.Add(new Vector2(aabb.X + aabb.Width, aabb.Y + aabb.Height));
            points.Add(new Vector2(aabb.X, aabb.Y + aabb.Height));
            return CalcInertia(points);
        }

        private float CalcInertia(List<Vector2> _points)
        {
            double sum1 = 0;
            double sum2 = 0;
            for (int i = 0; i < _points.Count - 1; i++)
            {
                float part1 = _points[i].X * _points[i].X + _points[i].Y * _points[i].Y +
                              _points[i].X * _points[i + 1].X + _points[i].Y * _points[i + 1].Y +
                              _points[i + 1].X * _points[i + 1].X + _points[i + 1].Y * _points[i + 1].Y;

                float part2 = _points[i].X * _points[i + 1].Y - _points[i + 1].X * _points[i].Y;

                sum1 += part1 * part2;
                sum2 += part2;
            }

            return (Density / 6) * (float)(sum1 / sum2);
        }

        /// <summary>
        /// Accelerates the rigidbody by the given force, not considering mass
        /// </summary>
        public void Accelerate(Vector2 _force)
        {
            acceleration += _force;
        }

        /// <summary>
        /// Accelerates the rigidbody by the given force
        /// </summary>
        public void ApplyLinearForce(Vector2 _force)
        {
            acceleration += _force / Mass;
        }

        /// <summary>
        /// Sets velocity directly
        /// </summary>
        public void ApplyLinearImpulse(Vector2 _velocity)
        {
            Velocity = _velocity;
        }

        /// <summary>
        /// Accelerates the rigidbody by the given force
        /// </summary>
        public void ApplyRotationalForce(float _force)
        {
            angularAcceleration += _force / Inertia;
        }

        /// <summary>
        /// Sets rotational velocity directly
        /// </summary>
        public void ApplyRotationalImpulse(float _velocity)
        {
            AngularVelocity = _velocity;
        }

        /// <summary>
        /// Sets all necessary vaiables to begin Integration
        /// </summary>
        public void StartIntegration()
        {
            newPosition = Position;
            newVelocity = Velocity;
            newAcceleration = acceleration;


            newRotation = gameObject != null ? gameObject.transform.Rotation : 0;
            newAngularVelocity = AngularVelocity;
            newAngularAcceleration = angularAcceleration;

            inFrame = true;
        }

        /// <summary>
        /// Integrates velocity and position for the time _timeDiff
        /// </summary>
        public void IntegrateTimestep(float _timeDiff)
        {
            MathUtils.IntegrateRK4(ref newAcceleration, ref newVelocity, ref newPosition, _timeDiff);
            MathUtils.IntegrateVerlet(ref newAngularAcceleration, ref newAngularVelocity, ref newRotation, _timeDiff);
        }

        /// <summary>
        /// Applies this frames results
        /// </summary>
        public void EndFrame()
        {
            Position = newPosition;
            Velocity = newVelocity;
            acceleration = Vector2.Zero;

            if(gameObject != null)
                gameObject.transform.Rotation = newRotation;
            AngularVelocity = newAngularVelocity;
            angularAcceleration = 0;
            
            inFrame = false;
        }
    }
}
